package bcit.nxt.mission;

// TurtleSquare.java
import gin.and.tonic.*;
import lejos.nxt.Button;
import lejos.nxt.ButtonListener;


public class Missionalign implements UltrasonicListener, TouchListener,ButtonListener
{

    private DragonRobot robot = new DragonRobot();
    int count=0;

    Missionalign()
    {
        UltrasonicSensor us = new UltrasonicSensor(SensorPort.S1);
        TouchSensor ts = new TouchSensor(SensorPort.S2);
        Motor arm = new Motor(MotorPort.A);
        robot.addPart(us);
        robot.addPart(ts);
        //us.addUltrasonicListener(this,20);
        //  ts.addTouchListener(this);
        arm.setSpeed(20);
        robot.addPart(arm);


        /*Misson module*/
        //robot.setTurtleSpeed(80);
        robot.forward(1215);//051911,let the tail touch the wall//1170);
        robot.left(16);
        robot.forward(530);//051611//480);
        arm.rotateTo(157);
        robot.right(110);    //finish mission bo gan
        robot.forward(70);//051611//120);
        arm.rotateTo(-39);//051611//-33);
        robot.right(100);   //finish mission bo xiao kuai
        robot.forward(410);
        robot.left(60);
        arm.rotateTo(-129);
        robot.forward(1400);    //start to push
       // robot.right(90);    //start to cross fields
        robot.exit();




        /*cross for NANO to POWER base*/
       /*robot.setTurtleSpeed(50);
         robot.forward(211);
         robot.left(70);
         robot.setTurtleSpeed(80);
         robot.forward(170);
         robot.setTurtleSpeed(50);
         robot.left(45);
         robot.forward(84);
         robot.right(45);*/
         //robot.forward(100);
        // robot.exit();


        /*from Power to NANO*/
        //robot.setTurtleSpeed(80);
        //robot.getGear().setSpeed(80);
        //robot.forward(1000);
        //robot.right(360);
        //robot.forward(100);


        //robot.left(360);
        //robot.forward(100);

        /*robot.getGear().right();
         robot.forward(422);
         robot.setTurtleSpeed(80);
         robot.right(90);
         robot.getGear().forward();*/


         /*sensor*/
        /* robot.getGear().forward();*/


    }

    public void far(SensorPort port, int level)
    {
        robot.getGear().forward();
    }

    public void near(SensorPort port, int level)
    {
      if(count==2)
      {
       count++;
       return;

      }

      if(count>=3)
      {
          count++;
          robot.right(90);
      }



       if(count<2)
       {
           count++;
           robot.left(90);
        }
      if(count==2)
      {
           robot.forward(63);
           robot.right(90);
           robot.forward(40);
           robot.right(90);
           robot.getGear().forward();
       }

    }

    public void pressed(SensorPort port)
    {
        robot.getGear().forward(200);
        robot.getGear().right();

    }

    public void released(SensorPort port)
    {
        robot.getGear().backward();
    }

     public void buttonPressed(Button b)
  {
    robot.exit();
  }

  public void buttonReleased(Button b)
  {
  }
    public static void main(String[] args)
    {

        new Missionalign();
    }
}
